#ifndef CLUSTERING_SPEED_TEST_CLUSTER_BASE_H
#define CLUSTERING_SPEED_TEST_CLUSTER_BASE_H

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PointIndices.h>
#include <pcl_conversions/pcl_conversions.h>

#include <clustering_speed_test/tic_toc.h>

#include <glog/logging.h>
#include <yaml-cpp/yaml.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>

using Point = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<Point>;
using PointCloudPtr = PointCloud::Ptr;

class Cluster_base {
 public:
  Cluster_base(ros::NodeHandle& _nh,YAML::Node& _node) {
    nh = _nh;
    node = _node;
    sub_cloud = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points",100,&Cluster_base::cloud_callback,this);
    LOG(INFO) << "cluster base inited.";
  }
  virtual void clustering(const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices) = 0;
  void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& _msg_in) {
    LOG(INFO) << "cloud recieved.";
    //TODO 将ros 点云转化成pcl点云
    PointCloudPtr cloud_in(new PointCloud);
    pcl::fromROSMsg(*_msg_in,*cloud_in);

    TicToc tic;
    //调用clustering函数
    std::vector<pcl::PointIndices> cluster_indices;
    this->clustering(cloud_in,cluster_indices);

    double time_used = tic.toc();
    LOG(INFO) << "time_used : " << time_used;
  }
  YAML::Node node;
  ros::NodeHandle nh;
  ros::Subscriber sub_cloud;
};

#endif 